import BruteForceBroadPhase from "./broad-phase/brute-force-broad-phase";
import ContactManager from "./constraint/contact/contact-manager";
import RayCastWrapper from "./common/ray-cast-wrapper";
import ConvexCastWrapper from "./common/convex-cast-wrapper";
import AabbTestWrapper from "./common/aabb-test-wrapper";
import Island from "./common/island";
import CONSTANT from "./constant";
import TimeStep from "./common/time-step";
import Utils from "../general/utils";
import InfoDisplay from "./common/info-display";
import BvhBroadPhase from "./broad-phase/bvh-broad-phase/bvh-broad-phase";
import Method from "./common/method";

export default class World {
    constructor(optional={}) {
        this._initVariable(optional);
    }

    //.................................................public.....................................................

    set simulate(state){
        switch (state) {
            case CONSTANT.SIMULATE_STATE_START:
                let _this = this;
                this._timer=Utils.requestAnimationFrame(()=>{
                    _this.step();
                },this._interval*1000,1000,this._timer);
                break;
            case CONSTANT.SIMULATE_STATE_STOP:
                Utils.cancelAnimationFrame(this._timer);
                break;
            case CONSTANT.SIMULATE_STATE_IMMEDIATELY:
                this.step();
                break;
            default:
                this.step();
                break;
        }
    }
    set interval(num){
        this._interval=num||0.1666;
    }
    get gravity() {
        return this._gravity;
    }
    set gravity(vec3) {
        Method.convertVec3(vec3,this._gravity);
    }
    set beforeSimulate(func){
        if(typeof func !== 'function')return;
        this._beforeCall=func;
    }
    set afterSimulate(func){
        if(typeof func !== 'function')return;
        this._afterCall=func;
    }
    get statsPanel(){
        return this._performance?this._performance.container:null;
    }

    step(timeStep) {
        if(this._beforeCall)this._beforeCall();
        if(this._performance)this._performance.setTime(0);
        let step=timeStep||this._interval;
        if(step!==this._lastInterval){
            if(this._timeStep.dt > 0) this._timeStep.dtRatio = step / this._timeStep.dt;
            this._timeStep.dt = step;
            this._timeStep.invDt = 1 / step;
            this._lastInterval=step;
        }
        this._updateContacts();
        this._solveIslands();
        for(let rgb = this._rigidBodyList; rgb != null; rgb = rgb._next){
            if(rgb._type===CONSTANT.RIGID_BODY_TYPE_STATIC||rgb._sleeping)continue;
            rgb.updateObject3Ds();
        }
        if(this._performance){
            this._performance.calcEnd();
            this._performance.update();
        }
        if(this._afterCall)this._afterCall();
    }
    addRigidBody(rigidBody) {
        if(rigidBody._world != null) {
            throw new Error("A rigid body cannot belong to multiple worlds.");
        }
        if(this._rigidBodyList == null) {
            this._rigidBodyList = rigidBody;
            this._rigidBodyListLast = rigidBody;
        } else {
            this._rigidBodyListLast._next = rigidBody;
            rigidBody._prev = this._rigidBodyListLast;
            this._rigidBodyListLast = rigidBody;
        }
        rigidBody._world = this;
        let s = rigidBody._shapeList;
        while(s != null) {
            let n = s._next;
            s._proxy = this._broadPhase.createProxy(s,s._aabb);
            s._id = this._shapeIdCount++;
            this._numShapes++;
            s = n;
        }
        this._numRigidBodies++;
    }
    removeRigidBody(rigidBody) {
        if(rigidBody._world !== this) {
            throw new Error("The rigid body doesn't belong to the world.");
        }
        let prev = rigidBody._prev;
        let next = rigidBody._next;
        if(prev != null) {
            prev._next = next;
        }
        if(next != null) {
            next._prev = prev;
        }
        if(rigidBody === this._rigidBodyList) {
            this._rigidBodyList = this._rigidBodyList._next;
        }
        if(rigidBody === this._rigidBodyListLast) {
            this._rigidBodyListLast = this._rigidBodyListLast._prev;
        }
        rigidBody._next = null;
        rigidBody._prev = null;
        rigidBody._world = null;
        let s = rigidBody._shapeList;
        while(s != null) {
            let n = s._next;
            this._broadPhase.destroyProxy(s._proxy);
            s._proxy = null;
            s._id = -1;
            let cl = s._rigidBody._contactLinkList;
            while(cl != null) {
                let n = cl._next;
                let c = cl._contact;
                if(c._s1 === s || c._s2 === s) {
                    let _this = cl._other;
                    _this._sleeping = false;
                    _this._sleepTime = 0;
                    let _this1 = this._contactManager;
                    let prev = c._prev;
                    let next = c._next;
                    if(prev != null) {
                        prev._next = next;
                    }
                    if(next != null) {
                        next._prev = prev;
                    }
                    if(c === _this1._contactList) {
                        _this1._contactList = _this1._contactList._next;
                    }
                    if(c === _this1._contactListLast) {
                        _this1._contactListLast = _this1._contactListLast._prev;
                    }
                    c._next = null;
                    c._prev = null;
                    if(c._touching) {
                        let cc1 = c._s1._contactCallback;
                        let cc2 = c._s2._contactCallback;
                        if(cc1 === cc2) {
                            cc2 = null;
                        }
                        if(cc1 != null) {
                            cc1.endContact(c);
                        }
                        if(cc2 != null) {
                            cc2.endContact(c);
                        }
                    }
                    let prev1 = c._link1._prev;
                    let next1 = c._link1._next;
                    if(prev1 != null) {
                        prev1._next = next1;
                    }
                    if(next1 != null) {
                        next1._prev = prev1;
                    }
                    if(c._link1 === c._b1._contactLinkList) {
                        c._b1._contactLinkList = c._b1._contactLinkList._next;
                    }
                    if(c._link1 === c._b1._contactLinkListLast) {
                        c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
                    }
                    c._link1._next = null;
                    c._link1._prev = null;
                    let prev2 = c._link2._prev;
                    let next2 = c._link2._next;
                    if(prev2 != null) {
                        prev2._next = next2;
                    }
                    if(next2 != null) {
                        next2._prev = prev2;
                    }
                    if(c._link2 === c._b2._contactLinkList) {
                        c._b2._contactLinkList = c._b2._contactLinkList._next;
                    }
                    if(c._link2 === c._b2._contactLinkListLast) {
                        c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
                    }
                    c._link2._next = null;
                    c._link2._prev = null;
                    c._b1._numContactLinks--;
                    c._b2._numContactLinks--;
                    c._link1._other = null;
                    c._link2._other = null;
                    c._link1._contact = null;
                    c._link2._contact = null;
                    c._s1 = null;
                    c._s2 = null;
                    c._b1 = null;
                    c._b2 = null;
                    c._touching = false;
                    c._cachedDetectorData._clear();
                    c._manifold._clear();
                    c._detector = null;
                    let _this2 = c._contactConstraint;
                    _this2._s1 = null;
                    _this2._s2 = null;
                    _this2._b1 = null;
                    _this2._b2 = null;
                    _this2._tf1 = null;
                    _this2._tf2 = null;
                    c._next = _this1._contactPool;
                    _this1._contactPool = c;
                    _this1._numContacts--;
                }
                cl = n;
            }
            this._numShapes--;
            s = n;
        }
        this._numRigidBodies--;
    }
    addJoint(joint) {
        if(joint._world != null) {
            throw new Error("A joint cannot belong to multiple worlds.");
        }
        if(this._jointList == null) {
            this._jointList = joint;
            this._jointListLast = joint;
        } else {
            this._jointListLast._next = joint;
            joint._prev = this._jointListLast;
            this._jointListLast = joint;
        }
        joint._world = this;
        joint._link1._other = joint._b2;
        joint._link2._other = joint._b1;
        if(joint._b1._jointLinkList == null) {
            joint._b1._jointLinkList = joint._link1;
            joint._b1._jointLinkListLast = joint._link1;
        } else {
            joint._b1._jointLinkListLast._next = joint._link1;
            joint._link1._prev = joint._b1._jointLinkListLast;
            joint._b1._jointLinkListLast = joint._link1;
        }
        if(joint._b2._jointLinkList == null) {
            joint._b2._jointLinkList = joint._link2;
            joint._b2._jointLinkListLast = joint._link2;
        } else {
            joint._b2._jointLinkListLast._next = joint._link2;
            joint._link2._prev = joint._b2._jointLinkListLast;
            joint._b2._jointLinkListLast = joint._link2;
        }
        joint._b1._numJointLinks++;
        joint._b2._numJointLinks++;
        let _this = joint._b1;
        _this._sleeping = false;
        _this._sleepTime = 0;
        let _this1 = joint._b2;
        _this1._sleeping = false;
        _this1._sleepTime = 0;
        joint._syncAnchors();
        this._numJoints++;
    }
    removeJoint(joint) {
        if(joint._world !== this) {
            throw new Error("The joint doesn't belong to the world.");
        }
        let prev = joint._prev;
        let next = joint._next;
        if(prev != null) {
            prev._next = next;
        }
        if(next != null) {
            next._prev = prev;
        }
        if(joint === this._jointList) {
            this._jointList = this._jointList._next;
        }
        if(joint === this._jointListLast) {
            this._jointListLast = this._jointListLast._prev;
        }
        joint._next = null;
        joint._prev = null;
        joint._world = null;
        let prev1 = joint._link1._prev;
        let next1 = joint._link1._next;
        if(prev1 != null) {
            prev1._next = next1;
        }
        if(next1 != null) {
            next1._prev = prev1;
        }
        if(joint._link1 === joint._b1._jointLinkList) {
            joint._b1._jointLinkList = joint._b1._jointLinkList._next;
        }
        if(joint._link1 === joint._b1._jointLinkListLast) {
            joint._b1._jointLinkListLast = joint._b1._jointLinkListLast._prev;
        }
        joint._link1._next = null;
        joint._link1._prev = null;
        let prev2 = joint._link2._prev;
        let next2 = joint._link2._next;
        if(prev2 != null) {
            prev2._next = next2;
        }
        if(next2 != null) {
            next2._prev = prev2;
        }
        if(joint._link2 === joint._b2._jointLinkList) {
            joint._b2._jointLinkList = joint._b2._jointLinkList._next;
        }
        if(joint._link2 === joint._b2._jointLinkListLast) {
            joint._b2._jointLinkListLast = joint._b2._jointLinkListLast._prev;
        }
        joint._link2._next = null;
        joint._link2._prev = null;
        joint._link1._other = null;
        joint._link2._other = null;
        joint._b1._numJointLinks--;
        joint._b2._numJointLinks--;
        let _this = joint._b1;
        _this._sleeping = false;
        _this._sleepTime = 0;
        let _this1 = joint._b2;
        _this1._sleeping = false;
        _this1._sleepTime = 0;
        this._numJoints--;
    }
    rayCast(_begin,_end,callback) {
        let begin=_begin.elements,end=_end.elements;
        let s = this._rayCastWrapper.begin.elements;
        s[0] = begin[0];s[1] = begin[1];s[2] = begin[2];
        let e = this._rayCastWrapper.end.elements;
        e[0] = end[0];e[1] = end[1];e[2] = end[2];
        this._rayCastWrapper.callback = callback;
        this._broadPhase.rayCast(begin,end,this._rayCastWrapper);
    }

    //.................................................private.....................................................

    _updateContacts() {
        if(this._performance)this._performance.setTime(1);
        this._contactManager._updateContacts();
        if(this._performance)this._performance.calcBroadPhase();
        let c = this._contactManager.contactList;
        while(c != null) {
            let n = c._next;
            if(!c._shouldBeSkipped) {
                c._updateManifold();
            }
            c = n;
        }
        if(this._performance)this._performance.calcNarrowPhase();
    }
    _solveIslands() {
        if(CONSTANT.SETTING_DISABLE_SLEEPING) {
            let b = this._rigidBodyList;
            while(b != null) {
                b._sleeping = false;
                b._sleepTime = 0;
                b = b._next;
            }
        }
        if(this._rigidBodyStack.length < this._numRigidBodies) {
            let newStackSize = this._rigidBodyStack.length << 1;
            while(newStackSize < this._numRigidBodies) newStackSize <<= 1;
            this._rigidBodyStack = new Array(newStackSize);
        }
        this._numIslands = 0;
        Method.copyElements(this._gravity.elements,this._island.gravity);
        let b = this._rigidBodyList;
        this._numSolversInIslands = 0;
        while(b != null) {
            let n = b._next;
            if(!(b._addedToIsland || b._sleeping || b._type === CONSTANT.RIGID_BODY_TYPE_STATIC)) {
                if(b._numContactLinks === 0 && b._numJointLinks === 0) {
                    this._island._stepSingleRigidBody(this._timeStep,b);
                    this._numIslands++;
                }else{
                    this.buildIsland(b);
                    this._island._step(this._timeStep,this._numVelocityIterations,this._numPositionIterations);
                    this._island._clear();
                    this._numIslands++;
                }
            }
            b = n;
        }
        this._contactManager._postSolve();
        b = this._rigidBodyList;
        while(b != null) {
            b._addedToIsland = false;
            b = b._next;
        }
        b = this._rigidBodyList;
        while(b != null) {
            let force=b.force,torque=b.torque;
            force[0] = 0;force[1] = 0;force[2] = 0;
            torque[0] = 0;torque[1] = 0;torque[2] = 0;
            b = b._next;
        }
        while(this._numSolversInIslands > 0) {
            this._solversInIslands[--this._numSolversInIslands]._addedToIsland = false;
            this._solversInIslands[this._numSolversInIslands] = null;
        }
        if(this._performance)this._performance.setTime(4);
    }
    buildIsland(base) {
        let stackCount = 1;
        this._island._addRigidBody(base);
        this._rigidBodyStack[0] = base;
        while(stackCount > 0) {
            let rb = this._rigidBodyStack[--stackCount];
            this._rigidBodyStack[stackCount] = null;
            if(rb._type === CONSTANT.RIGID_BODY_TYPE_STATIC) {
                continue;
            }
            let cl = rb._contactLinkList;
            while(cl != null) {
                let n = cl._next;
                let cc = cl._contact._contactConstraint;
                let ccs = cl._contact._contactConstraint._solver;
                if(cc.isTouching() && !ccs._addedToIsland) {
                    if(this._solversInIslands.length === this._numSolversInIslands) {
                        let newArray = new Array(this._numSolversInIslands << 1);
                        let _g = 0, _g1 = this._numSolversInIslands;
                        while(_g < _g1) {
                            let i = _g++;
                            newArray[i] = this._solversInIslands[i];
                            this._solversInIslands[i] = null;
                        }
                        this._solversInIslands = newArray;
                    }
                    this._solversInIslands[this._numSolversInIslands++] = ccs;
                    this._island._addConstraintSolver(ccs,cc._positionCorrectionAlgorithm);
                    let other = cl._other;
                    if(!other._addedToIsland) {
                        this._island._addRigidBody(other);
                        this._rigidBodyStack[stackCount++] = other;
                    }
                }
                cl = n;
            }
            let jl = rb._jointLinkList;
            while(jl != null) {
                let n = jl._next;
                let j = jl._joint;
                let js1 = j._solver;
                if(!js1._addedToIsland) {
                    if(this._solversInIslands.length === this._numSolversInIslands) {
                        let newArray = new Array(this._numSolversInIslands << 1);
                        let _g = 0, _g1 = this._numSolversInIslands;
                        while(_g < _g1) {
                            let i = _g++;
                            newArray[i] = this._solversInIslands[i];
                            this._solversInIslands[i] = null;
                        }
                        this._solversInIslands = newArray;
                    }
                    this._solversInIslands[this._numSolversInIslands++] = js1;
                    this._island._addConstraintSolver(js1,j._positionCorrectionAlgorithm);
                    let other = jl._other;
                    if(!other._addedToIsland) {
                        this._island._addRigidBody(other);
                        this._rigidBodyStack[stackCount++] = other;
                    }
                }
                jl = n;
            }
        }
    }
    convexCast(convex,begin,_translation,callback) {
        let translation=Method.convertVec3(_translation);
        this._convexCastWrapper.convex = convex;
        Method.copyElements(begin.elements,this._convexCastWrapper.begin.elements);
        Method.copyElements(translation.elements,this._convexCastWrapper.translation.elements);
        this._convexCastWrapper.callback = callback;
        this._broadPhase.convexCast(convex,begin,translation,this._convexCastWrapper);
    }
    aabbTest(aabb,callback) {
        this._aabbTestWrapper._aabb.copyFrom(aabb);
        this._aabbTestWrapper._callback = callback;
        this._broadPhase.aabbTest(aabb,this._aabbTestWrapper);
    }
    getRigidBodyList() {
        return this._rigidBodyList;
    }
    getJointList() {
        return this._jointList;
    }
    getBroadPhase() {
        return this._broadPhase;
    }
    getContactManager() {
        return this._contactManager;
    }
    getNumRigidBodies() {
        return this._numRigidBodies;
    }
    getNumJoints() {
        return this._numJoints;
    }
    getNumShapes() {
        return this._numShapes;
    }
    getNumIslands() {
        return this._numIslands;
    }
    getNumVelocityIterations() {
        return this._numVelocityIterations;
    }
    setNumVelocityIterations(numVelocityIterations) {
        this._numVelocityIterations = numVelocityIterations;
    }
    getNumPositionIterations() {
        return this._numPositionIterations;
    }
    setNumPositionIterations(numPositionIterations) {
        this._numPositionIterations = numPositionIterations;
    }

    _initVariable(optional){
        let broadPhaseType=optional.broadPhaseType||CONSTANT.BROAD_PHASE_TYPE_BVH;
        switch(broadPhaseType) {
            case CONSTANT.BROAD_PHASE_TYPE_BRUTE_FORCE:
                this._broadPhase = new BruteForceBroadPhase();
                break;
            case CONSTANT.BROAD_PHASE_TYPE_BVH:
                this._broadPhase=new BvhBroadPhase();
                break;
            default:
                console.error('world: have no such broad-phase-type-',broadPhaseType);
                break;
        }
        this._contactManager = new ContactManager(this._broadPhase);
        this._gravity =Method.convertVec3(optional.gravity||[0,-9.80665,0]);
        this._rigidBodyList = null;
        this._rigidBodyListLast = null;
        this._jointList = null;
        this._jointListLast = null;
        this._numRigidBodies = 0;
        this._numShapes = 0;
        this._numJoints = 0;
        this._numIslands = 0;
        this._numVelocityIterations = 10;
        this._numPositionIterations = 5;
        this._rayCastWrapper = new RayCastWrapper();
        this._convexCastWrapper = new ConvexCastWrapper();
        this._aabbTestWrapper = new AabbTestWrapper();
        this._island = new Island();
        this._solversInIslands = new Array(CONSTANT.SETTING_ISLAND_INITIAL_CONSTRAINT_ARRAY_SIZE);
        this._rigidBodyStack = new Array(CONSTANT.SETTING_ISLAND_INITIAL_RIGID_BODY_ARRAY_SIZE);
        this._timeStep = new TimeStep();
        this._shapeIdCount = 0;

        this._beforeCall=null;
        this._afterCall=null;
        this._timer={};
        this._interval=optional.interval||0.01666;
        this._lastInterval=0;
        this._performance =optional.stats?new InfoDisplay(this):false;
    }
}